{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Dynamics of 2-link manipulator\n",
    "\n",
    "![Spong book](figs/spong-2link.png)\n",
    "\n",
    "Reference:\n",
    "\n",
    "Robot Modeling and Control, Spong, Vidyasagar, Hutchinson, Wiley 2006\n",
    "\n",
    "We will choose the parameters to be"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from roboticstoolbox import DHRobot, RevoluteDH, ERobot, ELink, ETS\n",
    "from math import pi, sin, cos\n",
    "import numpy as np\n",
    "np.set_printoptions(linewidth=100, suppress=True)\n",
    "\n",
    "\n",
    "\n",
    "# link 1\n",
    "m1 = 1\n",
    "l1 = 1\n",
    "lc1 = 0.5\n",
    "\n",
    "# link 2\n",
    "m2 = 1\n",
    "l2 = 1\n",
    "lc2 = 0.5\n",
    "\n",
    "# joint configurations\n",
    "q1 = [0, 0]        # o-o- arm horizontal\n",
    "q2 = [0, pi/2]    # o-o| upper arm horizontal, lower arm vertical\n",
    "q3 = [pi/2, 0]    # o|o| arm vertical\n",
    "q4 = [pi/2, -pi/2] # o|o- upper arm upward, lower arm horizontal\n",
    "qq = [q1, q2, q3, q4]\n",
    "z = [0, 0]\n",
    "\n",
    "# global parameters\n",
    "g = 9.81\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Denavit-Hartenberg notation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "L1 = RevoluteDH(a=l1, m=m1, r=[-lc1, 0, 0])\n",
    "L2 = RevoluteDH(a=l2, m=m2, r=[-lc2, 0, 0])\n",
    "robot = DHRobot([L1, L2], gravity=[0, g, 0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The DH notation only allows rotation about the z-axis, which is out of the page in the Figure above.  Gravity acts downward so we set the gravity acceleration to be `[0, g, 0]` - accelerating upward at `g` provides the weight force experienced on Earth."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "┏━━━━┳━━━━━┳━━━━┳━━━━━━┓\n",
      "┃θⱼ  ┃ dⱼ  ┃ aⱼ ┃  ⍺ⱼ  ┃\n",
      "┣━━━━╋━━━━━╋━━━━╋━━━━━━┫\n",
      "┃ q1\u001b[0m ┃ 0.0\u001b[0m ┃  1\u001b[0m ┃ 0.0°\u001b[0m ┃\n",
      "┃ q2\u001b[0m ┃ 0.0\u001b[0m ┃  1\u001b[0m ┃ 0.0°\u001b[0m ┃\n",
      "┗━━━━┻━━━━━┻━━━━┻━━━━━━┛\n",
      "\n",
      "┌──────┬────┬──────────────┬────────────────────────┬────┬────┬────────┬────┐\n",
      "│  j   │ m  │      r       │           I            │ Jm │ B  │   Tc   │ G  │\n",
      "├──────┼────┼──────────────┼────────────────────────┼────┼────┼────────┼────┤\n",
      "│link1\u001b[0m │  1\u001b[0m │ -0.5,  0,  0\u001b[0m │  0,  0,  0,  0,  0,  0\u001b[0m │  0\u001b[0m │  0\u001b[0m │  0,  0\u001b[0m │  0\u001b[0m │\n",
      "│link2\u001b[0m │  1\u001b[0m │ -0.5,  0,  0\u001b[0m │  0,  0,  0,  0,  0,  0\u001b[0m │  0\u001b[0m │  0\u001b[0m │  0,  0\u001b[0m │  0\u001b[0m │\n",
      "└──────┴────┴──────────────┴────────────────────────┴────┴────┴────────┴────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "print(robot)\n",
    "print(robot.dyntable())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[19.62   4.905]\n",
      "[14.715  0.   ]\n",
      "[0. 0.]\n",
      "[4.905 4.905]\n"
     ]
    }
   ],
   "source": [
    "for q in qq:\n",
    "    tau = robot.gravload(q)\n",
    "    print(tau)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using equations (7.85) and (7.86) we can write an expression for gravity torque derived analytically using Euler-Lagrange (rather than Newton-Euler) approach"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def EL_grav(q): \n",
    "    return [(m1 * lc1 + m2 * l1) * g * cos(q[0]) + m2 * lc2 * g * cos(q[0] + q[1]), m2 * lc2 * g * cos(q[0] + q[1])]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[19.62   4.905] vs [19.62, 4.905]\n",
      "[14.715  0.   ] vs [14.715, 3.003446274908884e-16]\n",
      "[0. 0.] vs [1.2013785099635535e-15, 3.003446274908884e-16]\n",
      "[4.905 4.905] vs [4.905000000000001, 4.905]\n"
     ]
    }
   ],
   "source": [
    "for q in qq:\n",
    "    tau_rne = robot.gravload(q)\n",
    "    tau_el = EL_grav(q)\n",
    "    print(tau_rne, 'vs', tau_el)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In fact we could use symbolic values of the constants above, repeat the above process, and generate the symbolic equations for gravity load."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can explore the velocity coupling terms.  To isolate them we will set gravity and joint acceleration to zero"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-1.5,  0.5])"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.gravity = [0, 0, 0]\n",
    "robot.rne(q2, [1, 1], z)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using equations on page 261 and (7.87) we can write the velocity dependent terms as"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "def EL_velocity(q, qd):\n",
    "    h = -m2 * l1 * lc2 * sin(q[1])\n",
    "    c121 = h\n",
    "    c211 = c121\n",
    "    c221 = h\n",
    "    c112 = -h\n",
    "    c122 = 0\n",
    "    c222 = 0\n",
    "    return [c121 * qd[0] * qd[1] + c211 * qd[1] * qd[0] + c221 * qd[1] ** 2, c112 * qd[0] ** 2]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0. 0.] vs [-0.0, 0.0]\n",
      "[-1.5  0.5] vs [-1.5, 0.5]\n",
      "[0. 0.] vs [-0.0, 0.0]\n",
      "[ 1.5 -0.5] vs [1.5, -0.5]\n"
     ]
    }
   ],
   "source": [
    "qd = [1, 1]\n",
    "for q in qq:\n",
    "    tau_rne = robot.rne(q, qd, z)\n",
    "    tau_el = EL_velocity(q, qd)\n",
    "    print(tau_rne, 'vs', tau_el)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([1.0*g*m_1*(l_1 - l_c_1)*cos(q_0) + 1.0*l_1*m_2*(1.0*g*cos(q_0)*cos(q_1) - 1.0*(1.0*g*sin(q_0) - 1.0*l_1*qd_0**2)*sin(q_1))*cos(q_1) + 1.0*l_1*m_2*(1.0*g*sin(q_1)*cos(q_0) - l_2*(1.0*qd_0 + 1.0*qd_1)**2 + l_c_2*(1.0*qd_0 + 1.0*qd_1)**2 + (1.0*g*sin(q_0) - 1.0*l_1*qd_0**2)*cos(q_1))*sin(q_1) + 1.0*m_2*(l_2 - l_c_2)*(1.0*g*cos(q_0)*cos(q_1) - 1.0*(1.0*g*sin(q_0) - 1.0*l_1*qd_0**2)*sin(q_1)),\n",
       "       1.0*m_2*(l_2 - l_c_2)*(1.0*g*cos(q_0)*cos(q_1) - 1.0*(1.0*g*sin(q_0) - 1.0*l_1*qd_0**2)*sin(q_1))],\n",
       "      dtype=object)"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from spatialmath.base import symbol\n",
    "\n",
    "# link 1\n",
    "m1 = symbol('m_1')\n",
    "l1 = symbol('l_1')\n",
    "lc1 = symbol('l_c_1')\n",
    "\n",
    "# link 2\n",
    "m2 = symbol('m_2')\n",
    "l2 = symbol('l_2')\n",
    "lc2 = symbol('l_c_2')\n",
    "\n",
    "g = symbol('g')\n",
    "\n",
    "L1 = RevoluteDH(a=l1, m=m1, r=[-lc1, 0, 0])\n",
    "L2 = RevoluteDH(a=l2, m=m2, r=[-lc2, 0, 0])\n",
    "robot = DHRobot([L1, L2], gravity=[0, g, 0], symbolic=True)\n",
    "\n",
    "\n",
    "q = symbol('q_:2')\n",
    "qd = symbol('qd_:2')\n",
    "robot.rne_python(q, qd, [0, 0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## ERobot notation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will redefine the parameters of the model to numerical values"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "# link 1\n",
    "m1 = 1\n",
    "l1 = 1\n",
    "lc1 = 0.5\n",
    "\n",
    "# link 2\n",
    "m2 = 1\n",
    "l2 = 1\n",
    "lc2 = 0.5\n",
    "\n",
    "# joint configurations\n",
    "q1 = [0, 0]        # o-o- arm horizontal\n",
    "q2 = [0, pi/2]    # o-o| upper arm horizontal, lower arm vertical\n",
    "q3 = [pi/2, 0]    # o|o| arm vertical\n",
    "q4 = [pi/2, -pi/2] # o|o- upper arm upward, lower arm horizontal\n",
    "qq = [q1, q2, q3, q4]\n",
    "z = [0, 0]\n",
    "\n",
    "# global parameters\n",
    "g = 9.81"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "L1 = ELink(ets=ETS.rz(), m=1, r=[0.5, 0, 0], name='L1')\n",
    "L2 = ELink(ets=ETS.tx(1) * ETS.rz(), m=1, r=[0.5, 0, 0], parent=L1, name='L2')\n",
    "robot = ERobot([L1, L2], gravity=[0, g, 0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[19.62   4.905] vs [19.62, 4.905]\n",
      "[14.715  0.   ] vs [14.715, 3.003446274908884e-16]\n",
      "[0. 0.] vs [1.2013785099635535e-15, 3.003446274908884e-16]\n",
      "[4.905 4.905] vs [4.905000000000001, 4.905]\n"
     ]
    }
   ],
   "source": [
    "for q in qq:\n",
    "    tau_rne = robot.gravload(q)\n",
    "    tau_el = EL_grav(q)\n",
    "    print(tau_rne, 'vs', tau_el)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0.5"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
